package moro;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:
 * Copyright:    Copyright (c) 2001
 * Company:      Universit� di Bergamo
 * @author Brugali, Bleker, van Zutphen, Stellingwerf, Grobben
 * @version 1.0
 */

import java.awt.Graphics;
import java.io.PrintWriter;
import java.util.ArrayList;

public class Robot {
	String name;
	Position position;
	Platform platform;
	ArrayList<Device> sensors = new ArrayList<Device>();
	protected PrintWriter output = null;

        // marge tov obstacles
        final int marge  = 10;

        // correctie ten opzichte van het middelpunt van de robot
        final int corrRL = 20;
        final int corrF  = 30;
        final int corrB  = 20;

        
	public Robot(String name, double x, double y, double t, Environment environment) {
		this.name = name;
		position = new Position(x, y, Math.toRadians(t));
		platform = new Platform("P1", this, environment);
		sensors.add(new Laser("L1", this, new Position(20.0, 0.0, 0.0), environment));
		sensors.add(new Sonar("S1", this, new Position(20.0, 0.0, 0.0), environment));
	}

	public void readPosition(Position position) {
		synchronized(this.position) {
			this.position.copyTo(position);
		}
	}

	public void writePosition(Position position) {
		synchronized(this.position) {
			position.copyTo(this.position);
		}
	}

	public void paint(Graphics g) {
		platform.paint(g);
		for(Device sensor: sensors ) {
			sensor.paint(g);
		}
	}


	public void start() {
		platform.start();
		for(Device sensor: sensors) {
			sensor.start();
		}
	}

	protected synchronized void writeOut(String data) {
		if(output != null) {
                    output.println(data);
                }
		else {
                    System.out.println(this.name + " output not initialized");
                }
	}
        
        /**
         * verwerken van commandos ontvangen van Controller
         * parse en zend naar juiste Device
         * @param p_command
         * @return true indien dit is gelukt
         */
        public boolean sendCommand(String p_command) {
		int indexInit = p_command.indexOf(".");
		if(indexInit < 0) {
                    return false;
                }
                // haal devicenaam uit commando
		String deviceName = p_command.substring(0, indexInit);
		String command = p_command.substring(indexInit+1);

                // device gevonden (L1, S1, P1)  - vraag positie op
		if(deviceName.equals(this.name) && command.equalsIgnoreCase("GETPOS")) {
			writeOut("GETPOS X=" + position.getX() +
					" Y=" + position.getY() +
					" DIR=" + Math.toDegrees(position.getT()));
		}
                // device P1 ?
		else if(deviceName.equals(platform.name)) {
                    return platform.sendCommand(command);
                }
                // sensor devices L1, S1
		else {
                    for(Device sensor: sensors ) {
                        if(deviceName.equals(sensor.name)) {
                            return sensor.sendCommand(command);
                        }
                    }
                }
		return false;
	}

        /**
         * zend the informatie naar de devices
         * @param output
         */
        public void setOutput(PrintWriter output) {
		this.output = output;
		platform.setOutput(output);
		for(Device sensor: sensors ) {
			sensor.setOutput(output);
		}
	}


        /**
         * test methode
         */
        public void test() {
		platform.start();
		Laser laser = (Laser) sensors.get(0);
		laser.start();
		laser.sendCommand("L1.SCAN");
	}
        
        
        /**
         * haal referentie van index van Sensor in list
         * 
         * @param deviceName
         * @return index of sensor in list of sensors
         */
        public Device getSensorNameIndex(String deviceName) {
            int index=0;
            for(Device sensor: sensors ) {
                if(deviceName.equals(sensor.getName())) {
                    return sensors.get(index);
                }
                index++;
            }
            // dit mag niet voorkomen!
            return null;
        }

        /**
         * get noodzakelijke correcties voor de achterzijde robot
         * tov referentiepunt robot
         * @return afstand van referentiepunt tot achterzijde robot
         */
        public int getCorrB() {
            return corrB;
        }

        /**
         * get noodzakelijke correcties voor de voorzijde robot
         * tov referentiepunt robot
         * @return afstand van referentiepunt tot voorzijde robot
         */
        public int getCorrF() {
            return corrF;
        }

        /**
         * get noodzakelijke correcties voor de zijkanten robot
         * tov referentiepunt robot
         * @return afstand van referentiepunt tot zijkant van de robot
         */
        public int getCorrRL() {
            return corrRL;
        }

        /**
         * get noodzakelijke marge rondom de robot
         * 
         * @return minimumafstand van robot tot obstakels
         */
        public int getMarge() {
            return marge;
        }
         
}
